#include "claw_signal.h"
#include "claw.h"
#include "shooter.h"
#include "claw_motor.h"

uint16_t loosen_order = 0;       //放箭的顺序控制
uint16_t ga_mode_flag = 0;       //两种取箭方式（0取三支箭，1取剩余两支箭）
uint16_t ga_machine_flag = 1;    //状态机开启控制

/*开启抓手状态机标志*/
void Sign_OpenClawMachine()
{
    if (ga_machine_flag == 1) return;
    ga_machine_flag = 1;
    loosen_order = 0;     //TODO:放箭顺序归零
    ga_mode_flag = 0;     //TODO:设定取箭方式
    ga_status = GA_START;
    Signal_CleanClaw();
    uprintf("open claw machine!\r\n");
}

/*强制关闭抓手状态机标志*/
void Sign_CloseClawMachine()
{
    //TODO:关闭后无法重新启动
    ga_machine_flag = 0;
    ga_status = GA_START;
    ClawMotorReset();  //上升、翻转、移动三个电机homing
    uprintf("close claw machine!\r\n");
}

/*控制开始抓箭信号量*/
void Signal_StartTakeArrow()
{
    if (ga_status == GA_START) {
        osEventFlagsSet(EventTakeArrowHandle, TAKE_ARROW_START);
        shooter_restart(shooter1);

        uprintf("start taking arrows!\r\n");
    } else {
        uprintf("can not start taking arrows!\r\n");
    }
}

/*控制抓手左右移动信号量*/
void Signal_MoveClaw()
{
    // if (ga_status == GA_MOVE) {
        osEventFlagsSet(EventTakeArrowHandle, CLAW_MOVE);
        // uprintf("start moving claw!\r\n");
    // } else {
        // uprintf("can not start moving claw!\r\n");
    // }
}

/*控制抓手开合信号量*/
void Signal_OpenClaw()
{
    // if (ga_status == GA_LOOSEN) {
        osEventFlagsSet(EventTakeArrowHandle, CLAW_OPEN);
        // uprintf("start opening claw!\r\n");
    // } else {
        // uprintf("can not start opening claw!\r\n");
    // }
}

/*清除抓手所有信号量*/
void Signal_CleanClaw()
{
    osEventFlagsClear(EventTakeArrowHandle, TAKE_ARROW_START|CLAW_OPEN|CLAW_MOVE|
    RISE_GET_POSITION|RISE_FINSIH_HOMING|ROLL_GET_POSITION|ROLL_FINSIH_HOMING|MOVE_GET_POSITION|MOVE_FINISH_HOMING); //清空所有信号量
    uprintf("Clean all claw signal!\r\n");
}

/*取箭机构大疆电机位置反馈处理函数*/
void Claw_DjiPosOK(int16_t boardid, int16_t motorid)
{
    if (boardid == ClawBoardID) {
        switch(motorid) {
        case ClawRiseID:
            if (ga_status==GA_RISE || ga_status==GA_LOOSEN || ga_status==GA_RESET || ga_status==GA_MOVE || ga_status==GA_START)
                osEventFlagsSet(EventTakeArrowHandle, RISE_GET_POSITION);
            break;
        case ClawRollID:
            if (ga_status==GA_RESET || ga_status==GA_RISE)
                osEventFlagsSet(EventTakeArrowHandle, ROLL_GET_POSITION);
            break;
        case ClawMoveID:
            if (ga_status==GA_START || ga_status==GA_MOVE || ga_status==GA_LOOSEN) {
                uprintf("Set Signal Claw Move EVENT=%x\r\n", osEventFlagsSet(EventTakeArrowHandle, MOVE_GET_POSITION));
            }
            break;
        }
        uprintf("claw motor %d arrive target ok!\r\n", motorid);
    }
}

/*取箭机构大疆电机homing反馈处理函数*/
void Claw_DjiResetOK(int16_t boardid, int16_t motorid)
{
    if (boardid == ClawBoardID) {
        switch(motorid) {
        case ClawRiseID:
            osEventFlagsSet(EventTakeArrowHandle, RISE_FINSIH_HOMING);
            break;
        case ClawRollID:
            if (ga_status==GA_ROLL || ga_status==GA_RESET || ga_status==GA_START)
                osEventFlagsSet(EventTakeArrowHandle, ROLL_FINSIH_HOMING);
            break;
        case ClawMoveID:
            if (ga_status==GA_RESET || ga_status==GA_START)
                osEventFlagsSet(EventTakeArrowHandle, MOVE_FINISH_HOMING);
            break;
        }
        uprintf("claw motor %d homing ok!\r\n", motorid);
    }
}